#include "controller.h"
#include "../alu/gates.h"
#include <stdlib.h>
#include <stdio.h>

/*
 * Decodes the fetched instruction in the controller.
 * controller: a pointer to the controller being operated on.
 */
void decode(Controller *controller)
{
	controller->current_instruction =
		(cpu_get_ir(controller->cpu) & OPCODE_BITMASK) >> OPCODE_RSHIFT;
}

/*
 * Evaluates the address for a pcoffset instruction.
 * controller: a pointer to the controller being operated on.
 */
void evaluate_address_brpcoffset(Controller *controller)
{
	alu_set_operand_a(controller->alu, cpu_get_pc(controller->cpu));
	Register offset = sign_extend(cpu_get_ir(controller->cpu) & BRPCOFFSET_BITMASK, BRPCOFFSET_BITMASK);
	alu_set_operand_b(controller->alu, offset);
	alu_add(controller->alu);
	controller->computed_memory_address = alu_get_result_r(controller->alu);
}

/*
 * Evaluates the address for a pcoffset instruction.
 * controller: a pointer to the controller being operated on.
 */
void evaluate_address_pcoffset(Controller *controller)
{
	alu_set_operand_a(controller->alu, cpu_get_pc(controller->cpu));
	Register offset = sign_extend(cpu_get_ir(controller->cpu) & PCOFFSET_BITMASK, PCOFFSET_BITMASK);
	alu_set_operand_b(controller->alu, offset);
	alu_add(controller->alu);
	controller->computed_memory_address = alu_get_result_r(controller->alu);
}

/*
 * Evaluates the address for a register offset instruction.
 * controller: a pointer to the controller being operated on.
 */
void evaluate_address_registeroffset(Controller *controller)
{
	int base_reg_index =
		(cpu_get_ir(controller->cpu) & BASE_REG_BITMASK) >> BASG_REG_RSHIFT;
	Register base_reg = registerset_get_register(controller->register_file, base_reg_index);
	alu_set_operand_a(controller->alu, base_reg);
	Register offset = sign_extend(cpu_get_ir(controller->cpu) & REGOFFSET_BITMASK, REGOFFSET_BITMASK);
	alu_set_operand_b(controller->alu, offset);
	alu_add(controller->alu);
	controller->computed_memory_address = alu_get_result_r(controller->alu);
}

/*
 * Evaluates the address in the instruction.
 * controller: a pointer to the controller being operated on.
 */
void evaluate_address(Controller *controller)
{
	switch (controller->current_instruction)
	{
		case OP_BR:
			evaluate_address_brpcoffset(controller);
			break;

		case OP_LD:
		case OP_LEA:
		case OP_ST:
			evaluate_address_pcoffset(controller);
			break;

		case OP_LDR:
		case OP_STR:
			evaluate_address_registeroffset(controller);
			break;
	}
}

/*
 * Updates the PSR registers in the CPU with the results of the last operation.
 * controller: a pointer to the controller being operated on.
 */
void save_psr(Controller *controller)
{
	Register psr = cpu_get_state(controller->cpu);

	Bit zero = (register_get(&controller->result) == 0) ? TRUE : FALSE;
	Bit negative = register_get_bit(&controller->result, REGISTER_BITS - 1);

	register_set_bit(&psr, P_BIT_POS, not(or(negative, zero)));
	register_set_bit(&psr, Z_BIT_POS, zero);
	register_set_bit(&psr, N_BIT_POS, negative);
	register_set_bit(&psr, O_BIT_POS, FALSE);
	register_set_bit(&psr, C_BIT_POS, FALSE);

	cpu_set_state(controller->cpu, psr);
}

/*
 * Performs an ADD operation.
 * controller: a pointer to the controller being operated on.
 */
void add_op(Controller *controller)
{
	if (cpu_get_ir(controller->cpu) & IMMFLAG_BITMASK)
		controller->source_2 =
			sign_extend(cpu_get_ir(controller->cpu) & IMM_BITMASK, IMM_BITMASK);

	alu_set_operand_a(controller->alu, controller->source_1);
	alu_set_operand_b(controller->alu, controller->source_2);
	alu_add(controller->alu);
	controller->result = alu_get_result_r(controller->alu);
}

/*
 * Performs an AND operation.
 * controller: a pointer to the controller being operated on.
 */
void and_op(Controller *controller)
{
	int i;

	if (cpu_get_ir(controller->cpu) & IMMFLAG_BITMASK)
		controller->source_2 =
			sign_extend(cpu_get_ir(controller->cpu) & IMM_BITMASK, IMM_BITMASK);

	for (i = 0; i < REGISTER_BITS; i++)
	{
		Bit a = register_get_bit(&controller->source_1, i);
		Bit b = register_get_bit(&controller->source_2, i);
		Bit r = and(a, b);
		register_set_bit(&controller->result, i, r);
	}

	save_psr(controller);
}

/*
 * Performs an BR operation.
 * controller: a pointer to the controller being operated on.
 */
void br_op(Controller *controller)
{
	int n_bit = cpu_get_ir(controller->cpu) & BR_N_BITMASK;
	int z_bit = cpu_get_ir(controller->cpu) & BR_Z_BITMASK;
	int p_bit = cpu_get_ir(controller->cpu) & BR_P_BITMASK;
	int c_bit = cpu_get_ir(controller->cpu) & BR_C_BITMASK;
	int o_bit = cpu_get_ir(controller->cpu) & BR_O_BITMASK;
	int jump = 0;

	if (!n_bit && !z_bit && !p_bit && !c_bit && !o_bit)
		n_bit = z_bit = p_bit = c_bit = o_bit = 1;

	if (n_bit && (cpu_get_state(controller->cpu) & N_BIT_POS))
		jump = 1;
	else if (z_bit && (cpu_get_state(controller->cpu) & Z_BIT_POS))
		jump = 1;
	else if (p_bit && (cpu_get_state(controller->cpu) & P_BIT_POS))
		jump = 1;
	else if (c_bit && (cpu_get_state(controller->cpu) & C_BIT_POS))
		jump = 1;
	else if (o_bit && (cpu_get_state(controller->cpu) & O_BIT_POS))
		jump = 1;

	if (jump)
		cpu_set_pc(controller->cpu, controller->computed_memory_address);
}

/*
 * Performs a HALT operation.
 * controller: a pointer to the controller being operated on.
 */
void halt_op(Controller *controller)
{
	cpu_halt(controller->cpu);
}

/*
 * Performs a JMP operation.
 * controller: a pointer to the controller being operated on.
 */
void jmp_op(Controller *controller)
{
	cpu_set_pc(controller->cpu, controller->source_1);
}

/*
 * Performs a LD operation.
 * controller: a pointer to the controller being operated on.
 */
void ld_op(Controller *controller)
{
	memory_set_memory_address_register(controller->memory, controller->computed_memory_address);
	memory_perform_read(controller->memory);
	controller->result = memory_get_memory_data_register(controller->memory);

	save_psr(controller);
}

/*
 * Performs a LDR operation.
 * controller: a pointer to the controller being operated on.
 */
void ldr_op(Controller *controller)
{
	memory_set_memory_address_register(controller->memory, controller->computed_memory_address);
	memory_perform_read(controller->memory);
	controller->result = memory_get_memory_data_register(controller->memory);

	save_psr(controller);
}

/*
 * Performs an LEA operation.
 * controller: a pointer to the controller being operated on.
 */
void lea_op(Controller *controller)
{
	controller->result = controller->computed_memory_address;

	save_psr(controller);
}

/*
 * Performs an NOT operation.
 * controller: a pointer to the controller being operated on.
 */
void not_op(Controller *controller)
{
	int i;

	for (i = 0; i < REGISTER_BITS; i++)
	{
		Bit a = register_get_bit(&controller->source_1, i);
		Bit r = not(a);
		register_set_bit(&controller->result, i, r);
	}

	save_psr(controller);
}

/*
 * Performs an ST operation.
 * controller: a pointer to the controller being operated on.
 */
void st_op(Controller *controller)
{
	memory_set_memory_address_register(controller->memory, controller->computed_memory_address);
	memory_set_memory_data_register(controller->memory, controller->source_1);
	memory_perform_write(controller->memory);
}

/*
 * Performs an STR operation.
 * controller: a pointer to the controller being operated on.
 */
void str_op(Controller *controller)
{
	memory_set_memory_address_register(controller->memory, controller->computed_memory_address);
	memory_set_memory_data_register(controller->memory, controller->source_1);
	memory_perform_write(controller->memory);
}

/*
 * Performs an SUB operation.
 * controller: a pointer to the controller being operated on.
 */
void sub_op(Controller *controller)
{
	Register temp = controller->source_1;

	controller->source_1 = controller->source_2;
	not_op(controller);
	controller->source_1 = controller->result;
	controller->source_2 = 1;
	add_op(controller);
	controller->source_1 = temp;
	controller->source_2 = controller->result;
	add_op(controller);
}

/*
 * Executes the instruction in the in the IR.
 * controller: a pointer to the controller being operated on.
 */
void execute(Controller *controller)
{
	switch (controller->current_instruction)
	{
		case OP_ADD:  add_op (controller); return;
		case OP_AND:  and_op (controller); return;
		case OP_BR:   br_op  (controller); return;
		case OP_HALT: halt_op(controller); return;
		case OP_JMP:  jmp_op (controller); return;
		case OP_LD:   ld_op  (controller); return;
		case OP_LDR:  ldr_op (controller); return;
		case OP_LEA:  lea_op (controller); return;
		case OP_NOT:  not_op (controller); return;
		case OP_ST:   st_op  (controller); return;
		case OP_STR:  str_op (controller); return;
		case OP_SUB:  sub_op (controller); return;
	}
}

/*
 * Fetches the next instruction from the address pointed by the PC and
 * increments the PC.
 * controller: a pointer to the controller being operated on.
 */
void fetch(Controller *controller)
{
	Register pc = cpu_get_pc(controller->cpu);
	memory_set_memory_address_register(controller->memory, pc++);
	cpu_set_pc(controller->cpu, pc);

	memory_perform_read(controller->memory);

	cpu_set_ir(controller->cpu, memory_get_memory_data_register(controller->memory));
}

/*
 * Fetches memory or registers into two immediate registers.
 * controller: a pointer to the controller being operated on.
 */
void fetch_operands(Controller *controller)
{
	int sr1_index;
	int sr2_index = (cpu_get_ir(controller->cpu) & SR2_BITMASK) >> SR2_RSHIFT;

	switch (controller->current_instruction)
	{
		case OP_ST:
		case OP_STR:
			sr1_index = (cpu_get_ir(controller->cpu) & DR_BITMASK) >> DR_RSHIFT;
			break;

		default:
			sr1_index = (cpu_get_ir(controller->cpu) & SR1_BITMASK) >> SR1_RSHIFT;
			break;
	}

	controller->source_1 = registerset_get_register(controller->register_file, sr1_index);
	controller->source_2 = registerset_get_register(controller->register_file, sr2_index);
}

/*
 * Puts the immediate result into the destination register or memory address.
 * controller: a pointer to the controller being operated on.
 */
void store_result(Controller *controller)
{
	int dr_index = (cpu_get_ir(controller->cpu) & DR_BITMASK) >> DR_RSHIFT;

	switch (controller->current_instruction)
	{
		case OP_BR:
		case OP_HALT:
		case OP_JMP:
		case OP_ST:
		case OP_STR:
			return; // No register storage required
	}

	registerset_set_register(controller->register_file, dr_index, controller->result);
}

/*
 * Initializes a new controller.
 * alu: A reference to the ALU.
 * memory: A reference to the memory bank.
 * register_set: A reference to the register set.
 */
Controller *new_controller(Cpu *cpu, Alu *alu, Memory *memory, RegisterSet *register_file)
{
	Controller *controller = (Controller *)malloc(sizeof(Controller));
	controller->cpu = cpu;
	controller->alu = alu;
	controller->memory = memory;
	controller->register_file = register_file;
	return controller;
}

/*
 * Steps through the next instruction.
 * controller: a pointer to the controller being operated on.
 */
void controller_step(Controller *controller)
{
	fetch(controller);
	decode(controller);
	evaluate_address(controller);
	fetch_operands(controller);
	execute(controller);
	store_result(controller);
}

/*
 * Frees the memory associated with this controller.
 */
void delete_controller(Controller *controller)
{
	free(controller);
}
